#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <geometry_msgs/Twist.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>
 #include <xrobot_control/joint_states.h>
 #include <moveit/move_group_interface/move_group_interface.h>      

class TrajectoryController
{
    #define JOINTS_NUM  xrobot_control::joint_states::JOINTS_NUM
public:

    explicit TrajectoryController(std::string robot);

    /**
     * @brief 控制机械臂直线移动到目标点
     * @param target 目标点
     * @note 目标是各个关节的最终位置
     */
    void ArmLinearMoveToTarget(std::vector<double>& target);

    /**
     * @brief 控制机械臂直线移动到目标点
     * @param target 目标点
     * @note 目标是末端姿态
     */
    void ArmLinearMoveToTarget(geometry_msgs::Pose& target);

    /**
     * @brief 控制机械臂快速移动到目标点
     * @param target 目标点
     * @note 目标是各个关节的最终位置
     */
    void ArmQuicklyMoveToTarget(std::vector<double>& target);

    /**
     * @brief 控制机械臂快速移动到目标点
     * @param target 目标点
     * @note 目标是末端姿态
     */
    void ArmQuicklyMoveToTarget(geometry_msgs::Pose& target);

    /**
     * @brief 控制机械臂某个关节运动
     * @param id 目标关节号（1-6）
     * @param  target 目标点
     * @note 目标是末端姿态
     */
    void SingleJointControl(std::string id, double& _target);

    /**
     * @brief 主控制函数
     * @param ctrl_Hz 控制频率
     */
    void Control(void)
    {
        // 开启单线程 
        ros::AsyncSpinner spinner(1);
        spinner.start();       
        
        // 控制机械臂和夹爪恢复默认状态
       geometry_msgs::Pose pose;
       ArmQuicklyMoveToTarget(pose);
       SetGripperStatus(0);
       sleep(1);

       // 初始化过程点
       Clear_MidPoint();

        // 轨迹规划初始化
        plan.trajectory_.joint_trajectory.joint_names = _arm.getJointNames();
        plan.start_state_.joint_state.name = _arm.getJointNames();
        plan.planning_time_ = 3;

        ROS_INFO("Successfully init joints Position ");

        // 关节状态初始化
        _pose.position.x = 250;    _pose.position.y = 0;  _pose.position.z= 200; 
        
        ros::Rate loop_rate(10);
        while(ros::ok())
        {
            // 延时等待
            loop_rate.sleep();

            // 查看中断函数
            ros::spinOnce();
        }

        // 关闭线程 
        ros::shutdown(); 
    }

    /**
     * @brief 设置夹爪状态
     * @param status  0 正常， 1 闭合，2 打开 
     */
    void SetGripperStatus(int status)
    {
        switch (status)
        {
        case 0: 
            SetGripperAngle(0);
            break;
        case 1:
            SetGripperAngle(-0.7);
            break;
        case 2:
             SetGripperAngle(0.3);
            break;
        default:
            break;
        }
        _end.move();
    }

    void SetGripperAngle(double _angle)
    {
        std::vector<double> target;
        target.push_back(_angle);
        target.push_back(-_angle);
        _end.setJointValueTarget(target);
        _end.move();
    }


private:
    // 机器人关节长度
    double l1 = 105;  double l2 = 96;  double l3 = 50.3 + 105;  double gripper_len = 105;

    // pitch角偏差
    double pitch_bias = 0.1;
    // 存储指令类型
    int  TrajType = 0;

    geometry_msgs::Pose _pose;

    // 存储过程点
     std::vector<trajectory_msgs::JointTrajectoryPoint> trajectory_points;

    // 轨迹规划
    moveit::planning_interface::MoveGroupInterface::Plan plan;

    // 机器人名称
    std::string  _robot;

    // 路径规划组
    moveit::planning_interface::MoveGroupInterface _arm;
    moveit::planning_interface::MoveGroupInterface _end;

    // 上位机命令订阅者
    ros::Subscriber  MonitorJoints_Suber;
    ros::Subscriber  MonitorEffector_Suber;
    ros::Subscriber  VisionCmd_Suber;
    ros::Subscriber  JoyStickCmd_Suber;

    ros::Publisher     Trajectory_Puber;

    // 回调函数
    void JointsControl_Callback(sensor_msgs::JointState msg);
    void EffectorControl_Callback(geometry_msgs::Pose msg);
    void VisionCmd_Callback(geometry_msgs::Pose msg);
    void JoyStickCmd_Callback(geometry_msgs::Pose msg);

    // 视觉等待姿态
    void Vision_Waiting(int type);

    // 视觉夹取命令
    void Vision_GripperTask(geometry_msgs::Pose cmd);

    // 视觉手势识别命令
    void Vision_HandTask(geometry_msgs::Pose cmd);

    // 机械臂以某个形状的轨迹运动
    void Vision_ShapeMove(int  shape);

    // 获取不同颜色方块的目标放置位置 1 红 2 黄 3 绿 4 蓝
    geometry_msgs::Pose Get_Colorbox_Displayment(int color)
    {
        geometry_msgs::Pose target;
        target.orientation.y = -1.57;
        target.orientation.x = 0;
        switch (color)
        {
        case 1:  
            target.position.x =  -80;
            target.position.y =  -100;
            target.position.z =  20;
            break;
        case 2:  
            target.position.x =  -20;
            target.position.y =  -100;
            target.position.z =   20;
            break;     
        case 3:  
            target.position.x = 80 ;
            target.position.y =  -100;
            target.position.z =   20;
            break;
        case 4:  
            // target.position.x =  100;
            // target.position.y = -100;
            // target.position.z =   0;
            target.position.x = 80 ;
            target.position.y =  -100;
            target.position.z =   20;
            break;  
        default:
            break;
        }
        return target;
    }

    // 相机坐标系到世界坐标系的转换
    geometry_msgs::Pose  CameraToWorld(geometry_msgs::Pose msg);
 
    // 笛卡尔空间到关节空间的解算
    std::vector<double> CartesianToJointSpec(double x, double y, double z, double pitch, double roll);

    // 计算贝塞尔曲线
    std::vector<trajectory_msgs::JointTrajectoryPoint> 
    Calculate_BezierTrajectory(std::vector<trajectory_msgs::JointTrajectoryPoint> points, int target_num);

    // 计算LSPB轨迹
    std::vector<trajectory_msgs::JointTrajectoryPoint> 
    Calculate_LSPBTrajectory(std::vector<trajectory_msgs::JointTrajectoryPoint> points, int diff_num);    

    // 计算样条插值轨迹
    std::vector<trajectory_msgs::JointTrajectoryPoint> 
    Calculate_SplineTrajectory(std::vector<trajectory_msgs::JointTrajectoryPoint> points, int diff_num);        

    // 计算笛卡尔空间的轨迹
    std::vector<trajectory_msgs::JointTrajectoryPoint> 
    Calculate_CartesianTrajectory(std::vector<trajectory_msgs::JointTrajectoryPoint> points, int diff_num);

    // 笛卡尔空间到关节空间的解算
    std::vector<double> CartesianToJointSpec(geometry_msgs::Pose msg)
    {
        return CartesianToJointSpec(msg.position.x, msg.position.y, msg.position.z, msg.orientation.y, msg.orientation.x);
    }

    // 关节空间到笛卡尔空间的结算
    geometry_msgs::Pose JointSpecToCartesian( std::vector<double> js);

    // 添加中间点
    void Add_MidPoint(trajectory_msgs::JointTrajectoryPoint p)
    {
        trajectory_points.push_back(p);
    }

    void Add_MidPoint(sensor_msgs::JointState msg)
    {
         trajectory_msgs::JointTrajectoryPoint point;
        point.positions = msg.position;
        trajectory_points.push_back(point);           
    }

    void Add_MidPoint(geometry_msgs::Pose msg)
    {
        std::vector<double> _target = CartesianToJointSpec(msg);
        trajectory_msgs::JointTrajectoryPoint point;
        point.positions = _target;
        trajectory_points.push_back(point);
    }
    void Add_MidPoint(std::vector<double> msg)
    {
        trajectory_msgs::JointTrajectoryPoint point;
        point.positions = msg;
        trajectory_points.push_back(point);
    }

    void Clear_MidPoint(void)
    {
        // 清除已有过程点
        trajectory_points.clear();

        // 压入起始点
        trajectory_msgs::JointTrajectoryPoint point;
        point.positions = _arm.getCurrentJointValues();
        Add_MidPoint(point);
    }
private:

    // 求阶乘
    int factorial(int n)
    {
        int out = 1;
        for(int i = 1; i <= n; i ++)
        {
            out *= i;
        }
        return out;
    }

    // 统计学组合数计算
    int Calculate_CombinatorialNumber(int n, int i)
    {
        int out;
        out = factorial(n);
        out /= factorial(i) * factorial(n-i);
    }

    //三角函数限制函数
    double TC(double in)
    {
        if(in >= 1.) in = 1;
        else if(in <= -1.) in = -1;
        return in;
    }

    // 符号函数
    int sign(int in)
    {
        if(in < 0) in = -1;
        else if(in >= 0) in = 1;
        return in;
    }

    //限幅函数
    double Restriction(double in , double min, double max)
    {
        if(in > max) return max;
        else if(in < min) return min;
        else return in;
    }
};
